#include "Physics.h"

namespace GenBen {

/*
Each interval end must be greater or equal to start!

Return values:
0 - no intersection:
    1: ---
	2:     ---

1 - end of first interval is end of intersection:
    1: ---
	2:  ---

2 - beginning of first interval is beginning of intersection:
    1:  ---
	2: ---

3 - intersection is first interval (1 AND 2):
    1:  ---
	2: -----

4 - intersection is second interval:
    1: -----
	2:  ---
*/
uint intersectIntervals(float i1_start, float i1_end, float i2_start, float i2_end)
{
	ASSERT(i1_start <= i1_end);
	ASSERT(i2_start <= i2_end);

	if(i1_end <= i2_start || i2_end <= i1_start)
		return 0;

	if(i1_start >= i2_start && i2_end <= i1_start)
		return 3;

	if(i2_start >= i1_start && i1_end <= i2_start)
		return 4;

	if(i2_end >= i1_end)
		return 1;
	return 2;
}

RectCollision collideRect(const RectF& rect1, const RectF& rect2)
{
	float ix1_start = rect1.min.x;
	float ix1_end = rect1.max.x;
	float ix2_start = rect2.min.x;
	float ix2_end = rect2.max.x;

	float iy1_start = rect1.min.y;
	float iy1_end = rect1.max.y;
	float iy2_start = rect2.min.y;
	float iy2_end = rect2.max.y;

	uint x_intersection = intersectIntervals(ix1_start, ix1_end, ix2_start, ix2_end);
	uint y_intersection = intersectIntervals(iy1_start, iy1_end, iy2_start, iy2_end);

	uint result = NO_COLLISION;
	
	// Both axes must intersect for rectangles to collide
	if(!x_intersection || !y_intersection)
		return (RectCollision)result;

	if(x_intersection == 1)
		result |= COLLIDE_LEFT;
	if(x_intersection == 2)
		result |= COLLIDE_RIGHT;
	if(x_intersection == 4)
		result |= COLLIDE_LEFT | COLLIDE_RIGHT;

	if(y_intersection == 1)
		result |= COLLIDE_TOP;
	if(y_intersection == 2)
		result |= COLLIDE_BOTTOM;
	if(y_intersection == 4)
		result |= COLLIDE_TOP | COLLIDE_BOTTOM;
	
	return (RectCollision)result;
}

bool quickCollideRect(const RectF& rect1, const RectF& rect2)
{
	RectF rect = rect1;

	rect.min.x += QUICK_COLLIDE_PENETRATION;
	rect.min.y += QUICK_COLLIDE_PENETRATION;
	rect.max.x -= QUICK_COLLIDE_PENETRATION;
	rect.max.y -= QUICK_COLLIDE_PENETRATION;

	if(rect.max.x <= rect2.min.x || rect2.max.x <= rect.min.x)
		return false;
	if(rect.max.y <= rect2.min.y || rect2.max.y <= rect.min.y)
		return false;
	
	return true;
}

bool collideRects(const RectF& rect, uint n_rects, RectF* rects)
{
	ASSERT(rects);

	while(n_rects)
	{
		if(quickCollideRect(rect, rects[--n_rects]))
			return true;
	}
	return false;
}

/*
RectI multisampledCollision(const RectI& rect, const VectorI& shift, uint n_static_rects, RectI* static_rects)
{
	ASSERT(MAX_SAMPLE_SIZE >= 2);

	if(shift.x == 0 && shift.y == 0)
		return rect;

	int shift_x = shift.x;
	int shift_y = shift.y;

	int rect_x = rect.min.x;
	int rect_y = rect.min.y;

	int rect_final_x = rect_x + shift.x;
	int rect_final_y = rect_y + shift.y;

	while(rect_x != rect_final_x || rect_y != rect_final_y)
	{
		// Squared shift vector length
		int sqr_shift_len = (shift_x * shift_x + shift_y * shift_y);

		// Integer number of samples required to properly divide shift vector, squared.
		int n = sqr_shift_len / (MAX_SAMPLE_SIZE * MAX_SAMPLE_SIZE);
		if(n < 1)
		{
			if(collideRects(RectI(rect_x + shift_x, rect_y + shift_y, 
				rect_x + shift_x + rect.width(), rect_y + shift_y + rect.height()), n_static_rects, static_rects))
				break;
			
			rect_x += shift_x;
			rect_y += shift_y;
			
			continue;
		}

		// If real squared number of samples required to divide shift vector is not integer, round n up.
		if(n * (MAX_SAMPLE_SIZE * MAX_SAMPLE_SIZE) < sqr_shift_len)
			n++;

		int sample_x = shift_x > 0 ? (signed)isqrt((shift_x*shift_x) / n) : -(signed)isqrt((shift_x*shift_x) / n);
		int sample_y = shift_y > 0 ? (signed)isqrt((shift_y*shift_y) / n) : -(signed)isqrt((shift_y*shift_y) / n);

		if(collideRects(RectI(rect_x + sample_x, rect_y + sample_y, 
				rect_x + sample_x + rect.width(), rect_y + sample_y + rect.height()), n_static_rects, static_rects))
				break;
		
		rect_x += sample_x;
		rect_y += sample_y;

		shift_x -= sample_x;
		shift_y -= sample_y;
	}

	return RectI(rect_x, rect_y, rect_x + rect.width(), rect_y + rect.height());
}
*/

RectF multisampledCollision(const RectF& rect, const VectorF& shift, uint n_static_rects, RectF* static_rects)
{
	float shift_length = sqrtf(shift.x*shift.x + shift.y*shift.y);
	if(shift_length < 0.1f)
		return rect;
	float n_steps = ceil(shift_length / MAX_SAMPLE_SIZE);
	float x_step = shift.x / n_steps;
	float y_step = shift.y / n_steps;
	float x = rect.min.x;
	float y = rect.min.y;

	for(int i = int(n_steps); i > 0; --i)
	{
		x += x_step;
		y += y_step;

		if(collideRects(RectF(x, y, x + rect.width(), y + rect.height()), n_static_rects, static_rects))
		{
			x -= x_step;
			y -= y_step;
			break;
		}		
	}

	return RectF(x, y, x + rect.width(), y + rect.height());
}

/* 
Original python code for dividing vector into multiple samples:

def subdiv_vector(x, y, l):
	n = (x*x + y*y) / (l*l)
	if n < 1:
		print x, y
		return
	if n*l*l < (x*x + y*y):
		n += 1
	print isqrt((x*x) / n), isqrt((y*y) / n)
	subdiv_vector(x - isqrt((x*x) / n), y - isqrt((y*y) / n), l)

*/

RectF multisampledSlidingCollision(const RectF& rect, const VectorF& shift, uint n_static_rects, RectF* static_rects)
{
	ASSERT(MAX_SAMPLE_SIZE >= 2);

	// Optimize: implement this properly
	RectF result = multisampledCollision(rect, VectorF(shift.x, 0), n_static_rects, static_rects);
	result = multisampledCollision(result, VectorF(0, shift.y), n_static_rects, static_rects);

	return result;
}

} // namespace
